// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <opencv2/opencv.hpp>  // Include OpenCV API
#include <iostream>
 using namespace cv;
 using namespace rs2;
 using namespace std;
 double t=0;
int number=1;
VideoWriter rgb( "rgb_test6.avi", CV_FOURCC('M', 'J', 'P', 'G') , 25.0,Size(640,480));
VideoWriter mask_distance( "distance_test6.avi", CV_FOURCC('M', 'J', 'P', 'G') , 25.0,Size(640,480));
VideoWriter gary_distance( "gary_test6.avi", CV_FOURCC('M', 'J', 'P', 'G') , 25.0,Size(640,480));
	//VideoWriter _distance( "distance_test.avi", CV_FOURCC('M', 'J', 'P', 'G'), 25.0,Size(640,480));
Mat frame_to_mat(const rs2::frame& f)
{
   
   

    auto vf = f.as<video_frame>();
    const int w = vf.get_width();
    const int h = vf.get_height();

    if (f.get_profile().format() == RS2_FORMAT_BGR8)
    {
        cout<<"1"<<endl;
        return Mat(Size(w, h), CV_8UC3, (void*)f.get_data(), Mat::AUTO_STEP);
    }
    else if (f.get_profile().format() == RS2_FORMAT_RGB8)
    {
        cout<<"2"<<endl;
        auto r = Mat(Size(w, h), CV_8UC3, (void*)f.get_data(), Mat::AUTO_STEP);
        cv::cvtColor(r, r, CV_BGR2RGB);
        return r;
    }
    else if (f.get_profile().format() == RS2_FORMAT_Z16)
    {
        cout<<"3"<<endl;
        return Mat(Size(w, h), CV_16UC1, (void*)f.get_data(), Mat::AUTO_STEP);
    }
    else if (f.get_profile().format() == RS2_FORMAT_Y8)
    {
        cout<<"4"<<endl;
        return Mat(Size(w, h), CV_8UC1, (void*)f.get_data(), Mat::AUTO_STEP);;
    }

    throw std::runtime_error("Frame format is not supported yet!");
}

// Converts depth frame to a matrix of doubles with distances in meters
Mat depth_frame_to_meters(const rs2::pipeline& pipe, const rs2::depth_frame& f)
{
 

    Mat dm = frame_to_mat(f);
    dm.convertTo(dm, CV_32F);
   
    auto depth_scale = pipe.get_active_profile()
        .get_device()
        .first<depth_sensor>()
        .get_depth_scale();
    dm =  dm*depth_scale;
     Mat dcm;
     dcm= dm*20;
     dcm.convertTo(dcm, CV_8U);
    //cout<<dcm<<endl;
    return dcm;
}

    const int erosion_size = 4;
    const int distance_near=350/5;
    const int distance_far=450/5;
 



int main(int argc, char * argv[]) 
{
   
    colorizer colorize;
    rs2::align align_to(RS2_STREAM_COLOR);

    pipeline pipe;
    if(pipe.start())
    cout<<"ojbk"<<endl;
  

    uchar lutData[256];  
    for (int i = 0; i<256; i++)  
    {  
        if(i<distance_near)  
        lutData[i] =0;  
        if (i >= distance_near && i <= distance_far)  
            lutData[i] = 50+(i-distance_near)*10;  
        if (i > distance_far)  
            lutData[i] = 0;  
    }  

    Mat lut(1, 256, CV_8U, lutData);

    const auto window_name = "Display Image";
    namedWindow(window_name, WINDOW_AUTOSIZE);

    // We are using StructuringElement for erode / dilate operations
    auto gen_element = [](int erosion_size)
    {
        return getStructuringElement(cv::MORPH_RECT,
            cv::Size(erosion_size + 1, erosion_size + 1),
            cv::Point(erosion_size, erosion_size));
    };

       auto erode_less = gen_element(erosion_size);
    auto erode_more = gen_element(erosion_size * 2);

   

    
    auto create_mask_from_depth = [&](cv::Mat& depth)
    {
        Mat mask;
        LUT(depth,lut,mask);
       // inRange(depth, distance_near, distance_far,mask);
        //threshold(depth, depth, thresh, 255, type);
       // dilate(mask, mask, erode_less);	
       // erode(mask, mask, erode_more);
	medianBlur(mask,mask,3);
	   erode(mask, mask, erode_less);
           dilate(mask, mask, erode_more);
        return mask;
    };

    
    for (int i = 0; i < 10; i++) pipe.wait_for_frames();

    //frameset data;
    //frameset aligned_set;
    //frame depth;
   // Mat depth_mat,color_mat,mask;
    while (waitKey(1) < 0 && cvGetWindowHandle(window_name))
    {

        t = ((double)getTickCount() - t) / getTickFrequency()*1000;
		cout << "1time:" << t << endl;
		cout << "-----------------------------------------------------------" << endl;
		t = (double)getTickCount();
        frameset data = pipe.wait_for_frames();
        // Make sure the frameset is spatialy aligned 
        // (each pixel in depth image corresponds to the same pixel in the color image)
        frameset aligned_set = align_to.process(data);
        frame depth = aligned_set.get_depth_frame();
        
        auto depth_mat=depth_frame_to_meters(pipe,depth);

       auto color_mat = frame_to_mat(aligned_set.get_color_frame());
       
        auto mask= create_mask_from_depth(depth_mat);
        cvtColor(mask,mask,CV_GRAY2BGR);
        
        colorize.set_option(RS2_OPTION_COLOR_SCHEME, 2);
        frame bw_depth = colorize(depth);
        
        //  t = ((double)getTickCount() - t) / getTickFrequency()*1000;
		//cout << "2time:" << t << endl;
		//cout << "-----------------------------------------------------------" << endl;
		//putText(final_img, "t:"+to_string(t), Point(200, 200), 2, 2.0, Scalar(255, 255, 255), 2);
		//t = (double)getTickCount();
        // Generate "near" mask image:
       // auto great =frame_to_mat(_depth);
        Mat near = frame_to_mat(bw_depth);
        
       //imshow("1",near);
         cvtColor(near, near, CV_BGR2RGB);
        
     //   rgb<<color_mat;
       //     gary_distance<<near;
         //   mask_distance<<mask;
        imshow("mask",mask);
        
     



    }

    return EXIT_SUCCESS;
}
//catch (const rs2::error & e)
//{
//    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
//    return EXIT_FAILURE;
//}
//catch (const std::exception& e)
//{
//    std::cerr << e.what() << std::endl;
//    return EXIT_FAILURE;
//}






